#include <iostream>
#include <string>
#include "all.h"
#include <qpOASES.hpp>

using namespace std;
using namespace Eigen;

void test(){
    /*-----------------------步态调度器测试-----------------------------------------------------------------
//     float tStart = timetest();
    GaitScheduler gs;
    gs.setGait(2.0, 2.0, "trot");
    gs.calculateAll(3.14);

    for (int i = 0; i < 4; i++)
    {
        cout << "是否处于支撑相：" << gs.s_fai_[i] << "\t";
        cout << "支撑相相位进度：" << gs.t_st_norm_[i] << "\t";
        cout << "摆动相相位进度：" << gs.t_sw_norm_[i] << "\t";
        cout << endl;
    }
        // Sleep(1000);
     //    float tEnd = timetest();
     //    cout<<tEnd-tStart;
    -----------------------------------------------------------------------------------------------------*/

    /*-----------------------状态估计器测试----------------------------------------------------------------
//     float tStart = timetest();
    // for (int iii = 0; iii < 1000; iii++)
    // {
        StateEstimator se;
        Eigen::Matrix3d mat;

        mat << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;

        mat = R("x", 90);
        cout << mat << endl;

        Eigen::Vector3d vec;
        vec << 1,
            2,
            3;
        // cout << *se.getR_B_O();
        cout << "------------R_B_O------------" << endl
             << se.getR_B_O() << endl;

        cout << "------------a_com_B------------" << endl
             << se.geta_com_B(vec) << endl;
        cout << "------------a_com_O------------" << endl
             << se.geta_com_O() << endl;
        cout << "------------omega_OB_B------------" << endl
             << se.getomega_OB_B(vec) << endl;
        cout << "------------omega_OB_O------------" << endl
             << se.getomega_OB_O() << endl;

        cout << "------------omega_OBX_B------------" << endl
             << se.getomega_OBX_B() << endl;

        cout << "------------p_1_B------------" << endl
             << se.getp_i_B(1, vec) << endl;
        cout << "------------p_dot_1_B------------" << endl
             << se.getp_dot_i_B(1, vec, vec) << endl;

        cout << "------------x_k------------" << endl
             << se.getx_k(vec, vec) << endl;
    // }
//     float tEnd = timetest();
//     cout << tEnd - tStart;
    ------------------------------------------------------------------------------------------------*/

     // double v_x, v_y, omega_z;
     // Eigen::Matrix3d R_imu_imu0;

     // TrajectoryGenerator tg;
     // StateEstimator se;
     // Eigen::Vector<double, 13 * PREDICTION_HORIZON> vecD;
     // while(1){
     //      se.setIMUdata(R_imu_imu0);
     //      tg.setExpection(v_x, v_y, omega_z);
     //      tg.calculateAll((se.getR_B_O()));
     //      tg.getD(vecD);  
     // }

    /*-----------------------qpOASES测试-------------------------------------------------------------------

        // 1. 构建QP问题
    QProblem example(2, 1); // 优化变量, 约束变量 维度

    // 2. 初始化第一个QP
    real_t H[2*2] = {1.0, 0.0, 0.0, 0.5}; // Hessian Matrix
    real_t A[1*2] = {1.0, 1.0};                    // 不等式/等式约束系数矩阵
    real_t g[2] = {1.5, 1.0};                      // 线性项系数 与maatlab不同为 x' * g
    real_t lb[2] = {0.5, -2.0};                    // x 下界
    real_t ub[2] = {5.0, 2.0};                     // x 上界
    real_t lbA[1] = {-1.0};                             // 不等式/等式上界 上下界相等时为等式约束
    real_t ubA[1] = {2.0};                              // 不等式/等式下界

    int nWSR = 10;     

    example.init(H, g, A, lb, ub, lbA, ubA, nWSR); // 求解

    // 4. 得到最优解
    real_t xOpt[2];
    real_t yOpt[2+1];
    example.getPrimalSolution(xOpt);
    example.getDualSolution(yOpt);
    std::cout<<"PrimalSolution: "<<xOpt[0]<<","<<xOpt[1]<<std::endl; // 原问题最优变量
    std::cout<<"DualSolution: "<<yOpt[0]<<","<<yOpt[1]<<yOpt[2]<<","<<std::endl; // 最优对偶变量(优化变量个数+约束个数)
    std::cout<<"ObjVal: "<<example.getObjVal()<<std::endl; // 目标函数值
    ------------------------------------------------------------------------------------------------------*/

     // MPCsolver ms;
     // GaitPlanner gp;
     // StateEstimator se;
     TrajectoryGenerator tg;

     // gp.setGait(2, 2, "trot");
     // gp.calculateGait(3.14);
     
     tg.setExpection(50, 20, 10);
     Matrix3d RBO = R("x", 70)*R("y", 50);

     tg.calculateAll(&RBO); 

     Vector<double, 13*PREDICTION_HORIZON> vecD;
     tg.getD(vecD);    

     cout<<vecD<<endl;
     cout<<vecD.size();
}


int main()
{
     test();
}